#include "runge_kutta.hpp"

// 计算四元数的长度
double quaternionLength(Quaternion q) {
    return sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
}

// 归一化四元数
Quaternion normalizeQuaternion(Quaternion q) {
    double len = quaternionLength(q);
    Quaternion result = { q.w / len, q.x / len, q.y / len, q.z / len };
    return result;
}

// 计算四元数乘法
Quaternion quaternionMultiply(Quaternion q1, Quaternion q2) {
    Quaternion result;
    result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
    result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
    result.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
    result.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
    return result;
}

// 计算四元数的导数
Quaternion quaternionDerivative(Quaternion q, double wx, double wy, double wz) {
    Quaternion omega = { 0, wx, wy, wz }; // 将角速度转换四元数
    return quaternionMultiply(q, omega); // 计算导数
}

// 龙格-库塔算法更新四元数
// @param q     四元数
// @param wx    角速度 x 单位：弧度/秒
// @param wy    角速度 y 单位：弧度/秒
// @param wz    角速度 z 单位：弧度/秒
// @param h     时间步长 单位：秒
// @return      更新后的四元数
Quaternion updateQuaternion(Quaternion q, double wx, double wy, double wz, double h) {
    Quaternion k1, k2, k3, k4;

    // 计算 k1
    k1 = quaternionDerivative(q, wx, wy, wz);
    
    // 计算 k2
    Quaternion q2 = { q.w + h / 2 * k1.w, q.x + h / 2 * k1.x, 
                      q.y + h / 2 * k1.y, q.z + h / 2 * k1.z };
    k2 = quaternionDerivative(q2, wx, wy, wz);

    // 计算 k3
    Quaternion q3 = { q.w + h / 2 * k2.w, q.x + h / 2 * k2.x, 
                      q.y + h / 2 * k2.y, q.z + h / 2 * k2.z };
    k3 = quaternionDerivative(q3, wx, wy, wz);

    // 计算 k4
    Quaternion q4 = { q.w + h * k3.w, q.x + h * k3.x, 
                      q.y + h * k3.y, q.z + h * k3.z };
    k4 = quaternionDerivative(q4, wx, wy, wz);

    // 更新四元数
    Quaternion qNew;
    qNew.w = q.w + h / 6 * (k1.w + 2 * k2.w + 2 * k3.w + k4.w);
    qNew.x = q.x + h / 6 * (k1.x + 2 * k2.x + 2 * k3.x + k4.x);
    qNew.y = q.y + h / 6 * (k1.y + 2 * k2.y + 2 * k3.y + k4.y);
    qNew.z = q.z + h / 6 * (k1.z + 2 * k2.z + 2 * k3.z + k4.z);

    // 归一化
    return normalizeQuaternion(qNew);
}


// 欧拉角转换为四元数函数 可用于初始状态矫正
// 初始欧拉角可用重力加速度计测得
// @param roll  绕x轴旋转角度 单位：弧度
// @param pitch 绕y轴旋转角度 单位：弧度
// @param yaw   绕z轴旋转角度 单位：弧度
Quaternion eulerToQuaternion(double roll, double pitch, double yaw) {
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);

    Quaternion q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;

    return q;
}

/*加速度归一化*/
Acceleration normalizeacceleration ( Acceleration a)
{
    double len = sqrt(a.ax * a.ax + a.ay * a.ay + a.az * a.az);
    Acceleration result = { a.ax / len, a.ay / len, a.az / len };
    return result;
}

